#include "libXbee.h"
#include <sys/time.h>

//#define VOL

extern FILE *f;
extern unsigned int END;
extern int height;

#define THRUST_MAX_PREM_DROITE	2600 	/* 2600 : il bondit !!!	2700 il décolle	à vide	*/
#define THRUST_MAX_SEC_DROITE	2800 	/* 2600 : il bondit !!!	2700 il décolle	à vide 	*/

#define THRUST_MAINTIEN_1	2400 	/* 2400 : thrust de maintien à env. 2m. de hauteur 	*/

#define THRUST_MIN_TEST		50	/* seuil mini de thrust 			*/
#define TEMPS_MONTEE_1		12 	/* pente première droite de mise en vitesse en secondes */
#define TEMPS_MONTEE_2	        5 	/* pente deuxieme droite de mise en vitesse en secondes */
#define TEMPS_DESC_1		5 	/* pente de réduction de vitesse après le décollage */

#define TEMPS_STABILISE		3	/* temps de maitien à vitesse fixe 		*/ 
#define TEMPS_DESCENTE		8	/* temps de réduction de la vitesse en secondes */


int traceroute(){
  struct timeval tim;
  volatile double t5;
  volatile double t4;
  volatile double t3;
  volatile double t2;
  int height_moins_un;	

  gettimeofday(&tim, NULL);
  double t1=tim.tv_sec+(tim.tv_usec/1000000.0);

  printf(" temps initial %lf\n",t1);

	t2 = t3 = t4 = t5 = 0;

/* déplacement vers le haut premiere droite*/
  fprintf(f,"\t début accélération pente 1\n");
	do{
	usleep(100000); /* 100ms */
	add_thrust(50);
	gettimeofday(&tim, NULL);
	t2=tim.tv_sec+(tim.tv_usec/1000000.0);
        fprintf(f,"\t my_commanddata.thrust  = %d\n",my_commanddata.thrust);
        fprintf(f,"\t height  = %d \n         delta temps t2-t1 = %f\n",height, t2-t1);
	if( my_commanddata.thrust >= THRUST_MAX_PREM_DROITE )
		break;
	}while((t2-t1)<TEMPS_MONTEE_1 && !END);

  fprintf(f,"\t fin accélération pente 1\n");
  printf("%.6lf temps intermediaire 1\n", t2-t1);

/* déplacement vers le haut deuxieme droite*/
  fprintf(f,"\t début accélération pente 2\n");
	do{
	usleep(100000); /* 100ms */
	add_thrust(50);
	gettimeofday(&tim, NULL);
	t3=tim.tv_sec+(tim.tv_usec/1000000.0);
        fprintf(f,"\t my_commanddata.thrust  = %d\n",my_commanddata.thrust);
        fprintf(f,"\t height  = %d \n         delta temps t3-t2 = %f\n",height, t3-t2);
	if( my_commanddata.thrust >= THRUST_MAX_SEC_DROITE )
		break;
	}while((t3-t2)<TEMPS_MONTEE_2 && !END);
  fprintf(f,"\t fin accélération pente 2\n");
  printf("%.6lf temps intermediaire 2\n", t3-t2);

/* réduction thrust pour diminution hauteur */
  fprintf(f,"\t début décélération pente \n");
	do{
	usleep(100000); /* 100ms */
	sub_thrust(50);
	gettimeofday(&tim, NULL);
	t2=tim.tv_sec+(tim.tv_usec/1000000.0);
        fprintf(f,"\t my_commanddata.thrust  = %d\n",my_commanddata.thrust);
        fprintf(f,"\t height  = %d \n         delta temps t2-t3 = %f\n",height, t2-t3);
	if( my_commanddata.thrust >= THRUST_MAINTIEN_1 )
		break;
	}while((t2-t3)<TEMPS_DESC_1 && !END);
  fprintf(f,"\t fin décélération pente \n");
  printf("%.6lf temps intermediaire 3\n", t2-t3);

/* mémorisation valeur courante de hauteur */
  height_moins_un = height;

/* vol pdt 5 sec */
  fprintf(f,"\t début maintien\n");
	do{
	usleep(100000); /* 100ms */

#ifdef VOL

        fprintf(f,"\t height_moins_un  = %d\n",height_moins_un);
	if ((height != 0) && (height-height_moins_un > 150))
	{
	sub_thrust(20);
	}
	else 
	{
		if ((height != 0) && (height_moins_un-height > 150))
		{
		add_thrust(20);
		}
		else 
		{
		add_thrust(0);
		}
	}
	height_moins_un = height;

#endif

	add_thrust(0);
	gettimeofday(&tim, NULL);
	t4=tim.tv_sec+(tim.tv_usec/1000000.0);
        fprintf(f,"\t my_commanddata.thrust  = %d\n",my_commanddata.thrust);
        fprintf(f,"\t height  = %d \n         delta temps t4-t2 = %f\n",height, t4-t2);
	}while((t4-t2)<TEMPS_STABILISE && !END);
  fprintf(f,"\t fin maintien\n");

/* déplacement vers le bas temps de descente 10 s*/
  fprintf(f,"\t début décélération\n");
	do{
	usleep(100000); /* 100ms */
	sub_thrust(50);
	gettimeofday(&tim, NULL);
	t5=tim.tv_sec+(tim.tv_usec/1000000.0);
        fprintf(f,"\t my_commanddata.thrust  = %d\n",my_commanddata.thrust);
        fprintf(f,"\t height  = %d \n         delta temps t5-t4 = %f\n",height, t5-t4);
	if( my_commanddata.thrust < THRUST_MIN_TEST )
		break;
	}while((t5-t4)<TEMPS_DESCENTE  && !END);
  fprintf(f,"\t fin accélération\n");
  printf("%.6lf temps intermediaire 2\n", t5-t4);

  printf(" temps final %lf\n",t5);
  printf("%.6lf seconds elapsed\n", t5-t1);

return (EXIT_SUCCESS);

}


#if 0

#define TEMPS_ROT_A_DROITE  	3
#define VAL_YAW 		1024

int tracecarre(){
  struct timeval tim;
  volatile double t1;
  volatile double t2;
  volatile double t3;
  volatile double t4;


  gettimeofday(&tim, NULL);
  t1=tim.tv_sec+(tim.tv_usec/1000000.0);
printf(" temps initial %lf\n",t1);

/* déplacement vers l'avant */
	add_pitch(50);
	do{
	gettimeofday(&tim, NULL);
	t2=tim.tv_sec+(tim.tv_usec/1000000.0);
}while((t2-t1)<1);
  printf("%.6lf temps intermediaire 1\n", t2-t1);
	sub_pitch(50);/* stop avance */

/* rotation à droite */
	do{
	usleep(100000); /* 100ms */
	add_yaw(50);
	gettimeofday(&tim, NULL);
	t3=tim.tv_sec+(tim.tv_usec/1000000.0);
        fprintf(f,"\t my_commanddata.yaw  = %d\n",my_commanddata.yaw);
	if( my_commanddata.yaw >= VAL_YAW )
		break;
	}while((t3-t2)<TEMPS_ROT_A_DROITE && !END);

/* déplacement vers l'avant */
	add_pitch(50);
	do{
	gettimeofday(&tim, NULL);
	t2=tim.tv_sec+(tim.tv_usec/1000000.0);
}while((t2-t3)<1);
  printf("%.6lf temps intermediaire 2\n", t2-t3);

	sub_pitch(50);/* stop avance */

/* rotation à droite */
	do{
	usleep(100000); /* 100ms */
	add_yaw(50);
	gettimeofday(&tim, NULL);
	t3=tim.tv_sec+(tim.tv_usec/1000000.0);
        fprintf(f,"\t my_commanddata.yaw  = %d\n",my_commanddata.yaw);
	if( my_commanddata.yaw >= VAL_YAW )
		break;
	}while((t3-t2)<TEMPS_ROT_A_DROITE && !END);

/* déplacement vers l'avant */
	add_pitch(50);
	do{
	gettimeofday(&tim, NULL);
	t2=tim.tv_sec+(tim.tv_usec/1000000.0);
}while((t2-t3)<1);
  printf("%.6lf temps intermediaire 3\n", t2-t3);

	sub_pitch(50);/* stop avance */

/* rotation à droite */
	do{
	usleep(100000); /* 100ms */
	add_yaw(50);
	gettimeofday(&tim, NULL);
	t3=tim.tv_sec+(tim.tv_usec/1000000.0);
        fprintf(f,"\t my_commanddata.yaw  = %d\n",my_commanddata.yaw);
	if( my_commanddata.yaw >= VAL_YAW )
		break;
	}while((t3-t2)<TEMPS_ROT_A_DROITE && !END);

/* déplacement vers l'avant */
	add_pitch(50);
	do{
	gettimeofday(&tim, NULL);
	t2=tim.tv_sec+(tim.tv_usec/1000000.0);
}while((t2-t3)<1);
  printf("%.6lf temps intermediaire 4\n", t2-t3);
	sub_pitch(50);/* stop avance */

t4=tim.tv_sec+(tim.tv_usec/1000000.0);

printf(" temps final %lf\n",t4);
  printf("%.6lf seconds elapsed\n", t4-t1);
return (EXIT_SUCCESS);

}

#endif


